/**
*  \file control_pump.c
*
*  \brief
*
*  \author Michael J. Hohmann  <michael.hohmann@linde-mh.de>
*
*
*/

/*===============================================[ private includes  ]================================================*/
#include "control_pump.h"
#include "../app/app.h"
#include "../assertor/assertor.h"
#include "../pal/pal_mem.h"
#include "../cfg/control_pump_cfg.h"

#include "../interpol/interpol_s16_s16.h"
#include "../interpol/interpol_u16_u16.h"
#include "../mlib/max.h"
#include "../supervisor/supervisor.h"
#include "../output/output.h"
#include <stdlib.h>

//#ifdef UNIT_TEST
//	#include "../control_pump_unittest/control_pump_unittest.h"
//	#define static
//#else

/*===============================================[ private defines   ]================================================*/

#define LIFT_POT_CURVE_X_0		0
#define LIFT_POT_CURVE_X_1		1
#define LIFT_POT_CURVE_X_2		2
#define LIFT_POT_CURVE_X_3		3
#define LIFT_POT_CURVE_Y_0		4
#define LIFT_POT_CURVE_Y_1		5
#define LIFT_POT_CURVE_Y_2		6
#define LIFT_POT_CURVE_Y_3		7

#define SWITCH_FILT				5
#define LIFT_SWITCH_1_ON		((BO)(pump.input.lift_switch_1 > SWITCH_FILT))
#define LIFT_SWITCH_2_ON		((BO)(pump.input.lift_switch_2 > SWITCH_FILT))
#define TILT_SWITCH_ON			((BO)(pump.input.tilt_switch > SWITCH_FILT))
#define AUX1_SWITCH_ON			((BO)(pump.input.aux_switch_1 > SWITCH_FILT))
#define AUX2_SWITCH_ON			((BO)(pump.input.aux_switch_2 > SWITCH_FILT))
#define SEAT_SWITCH_ON			((BO)(pump.input.seat_switch_1 > SWITCH_FILT))

#define STEER_MOTOR_TORQUE_THRESHOLD	6

/*===============================================[ private datatypes ]================================================*/

typedef struct PUMP_INPUT
{
	S16 lift_switch_1;
	S16 lift_switch_2;
	S16 lift_potentio_ratio;
	S16 tilt_switch;
	S16 aux_switch_1;
	S16 aux_switch_2;
	S16 seat_switch_1;
	S16 SOC;

	S16 acc_pedal_ratio;
	S16 truck_speed;		//m/h
	S16 trac_regeneration_current;

	BO magnet_ready;
	F32 motor_torque;		//N.m
}PUMP_INPUT;

typedef enum LIFT_MODE
{
	invalid				=	0,
	one_lift_switch		= 	1,
	two_lift_switch		=	2,
	potentiometer		=	3
}LIFT_MODE;

typedef struct PUMP_PARAMETER
{
	S16 max_rpm;
	S16 steer_truck_speed_threshold;		//threshold to start pump for steering, m/h
	S16 steer_primary_rpm;			//low speed when torque is small
	S16 steer_secondary_rpm;		//high speed when torque is big
	S16 steer_motor_torque_threshold;		//torque threshold to switch pump motor rpm
	S16 steer_close_delay;
	S16 steer_ramp_up;
	S16 steer_ramp_down;

	LIFT_MODE lift_mode;
	S16 lift_1s_rpm;
	S16 lift_2s_rpm_1;
	S16 lift_2s_rpm_2;
	S16 lift_pot_min_prm;
	S16 lift_pot_max_rpm;
	S16 lift_ramp_up;
	S16 lift_ramp_down;

	S16 tilt_rpm;
	S16 tilt_ramp_up;
	S16 tilt_ramp_down;

	S16 aux1_rpm;
	S16 aux1_ramp_up;
	S16 aux1_ramp_down;

	S16 aux2_rpm;
	S16 aux2_ramp_up;
	S16 aux2_ramp_down;

	U8 soc_cutback_start;
	U8 soc_cutback_end;
	U8 soc_rpm_cutback;

	U8	motor_discharge_enable;
	S16 regenration_current_threshold;
	S16 soc_threshold;		//when soc > this threshold, and regeneration current > setting current, rotate pump with full speed to discharge
    U8 motor_temp_red_start;
    U8 motor_temp_red_end;
    U8 motor_temp_rpm_cutback;
    U8 chip_temp_red_start;
    U8 chip_temp_red_end;
    U8 chip_temp_rpm_cutback;
}PUMP_PARAMETER;

typedef struct PUMP_OUTPUT
{
	S16 final_motor_rpm;
	S16 motor_torque;
}PUMP_OUTPUT;

typedef struct PUMP
{
	PUMP_INPUT 		input;
	PUMP_PARAMETER	parameter;
	PUMP_OUTPUT		output;
}PUMP;

typedef struct  PUMP_FALG
{
    BO   steerFlag;
	BO   liftFlag;
	BO   tiltFlag;
	BO   aux1Flag;
	BO   aux2Flag;
}PUMP_FLAG;

//#endif
/*===============================================[ private variables ]================================================*/

static enum trilean disable_pump;

static PUMP_FLAG pumpF;
static F32 steer_current_rpm;
static F32 lift_current_rpm;
static F32 tilt_current_rpm;
static F32 aux1_current_rpm;
static F32 aux2_current_rpm;

static S16 steer_target_rpm;
static S16 lift_target_rpm;
static S16 tilt_target_rpm;
static S16 aux1_target_rpm;
static S16 aux2_target_rpm;

static S16 rpm_output_buffer;

static S16 lift_pot_curve_x[4];
static S16 lift_pot_curve_y[4];
static struct interpol_s16_s16_obj lift_potentio_curve;

static S16 batt_cutback_curve_x[4];
static S16 batt_cutback_curve_y[4];
static struct interpol_s16_s16_obj batt_cutback_curve;

static S16 cnt_1st_rpm_delay;
static S16 cnt_2nd_rpm_delay;

static S16 primary_target_rpm;
static S16 secondary_target_rpm;

static F32 step;
static S16 cnt2s;

PUMP pump;

/*===============================================[ public variables  ]================================================*/

S16 pump_motor_enable;			//enable the converter
S16 pump_motor_PWM_enable;		//enable the PWM (if OK, the magnetReady will be 1);

/*===============================================[ private functions ]================================================*/

static void control_pump_input_update();

static void control_pump_parameter_update();

static void pump_parameter_rx(U16 RxMsgData[4]);

static void pump_parameter_tx(U16 TxMsgData[4]);

static void control_pump_steer();

static void control_pump_lift();

static void control_pump_tilt();

static void control_pump_aux1();

static void control_pump_aux2();

static void ramp_control(S16 target_value, F32* current_value, S16 ramp_y, S16 ramp_up_x, S16 ramp_down_x);

static void output_max_rpm();

static void interpol_s16_s16_construct( struct interpol_s16_s16_obj *me, pal_size_t count, S16 const * p_x, S16 const * p_y );

static void batt_cutback_control();

static void pump_motor_discharge_control();

static void pump_motor_enable_control();

/*================================================[ inline functions ]================================================*/

/*================================================[ public functions ]================================================*/
RC control_pump_initialize(void)
{
	pal_memzero(&pump, sizeof(pump));
	pal_memzero(&pumpF, sizeof( pumpF));


	pal_memzero(lift_pot_curve_x, sizeof(lift_pot_curve_x));
	pal_memzero(lift_pot_curve_y, sizeof(lift_pot_curve_y));
	pal_memzero(&lift_potentio_curve, sizeof(lift_potentio_curve));

	pal_memzero(batt_cutback_curve_x, sizeof(batt_cutback_curve_x));
	pal_memzero(batt_cutback_curve_y, sizeof(batt_cutback_curve_y));
	pal_memzero(&batt_cutback_curve, sizeof(batt_cutback_curve));

	disable_pump 		= 	trilean_true;
	steer_current_rpm	=	0;
	lift_current_rpm	=	0;
	tilt_current_rpm	=	0;
	aux1_current_rpm	=	0;
	aux2_current_rpm	=	0;

	steer_target_rpm	=	0;
	lift_target_rpm		=	0;
	tilt_target_rpm		=	0;
	aux1_target_rpm		=	0;
	aux2_target_rpm		=	0;

	pump_motor_enable		=	0;
	pump_motor_PWM_enable	=	0;

	cnt_1st_rpm_delay = 0;
	cnt_2nd_rpm_delay = 0;
	primary_target_rpm = 0;
	secondary_target_rpm = 0;

	step = 0;
	cnt2s = 0;

	interpol_s16_s16_construct(&lift_potentio_curve, 4, lift_pot_curve_x, lift_pot_curve_y);
	interpol_s16_s16_construct(&batt_cutback_curve, 4, batt_cutback_curve_x, batt_cutback_curve_y);

	return RC_SUCCESS;
}

S16 control_pump_10ms()
{
	control_pump_input_update();
	control_pump_parameter_update();
	disable_pump = fDisPump;

	control_pump_steer();
	control_pump_lift();
	control_pump_tilt();
	control_pump_aux1();
	control_pump_aux2();

	output_max_rpm();
	batt_cutback_control();
	pump_motor_discharge_control();

	pump_motor_enable_control();

	return pump.output.final_motor_rpm;
}

S16 calculate_pump_torque_10ms()
{
	pump.output.motor_torque = 0;

	return pump.output.motor_torque;
}

/*===============================================[ private functions ]================================================*/

static void control_pump_input_update()
{
	pump.input.acc_pedal_ratio				=	input_data.accelerator;
	pump.input.truck_speed					=	output_data.te_vSen * 1000;		//te_vSen unit: Km/h

	pump.input.lift_switch_1				=	input_data.lift_switch_1;
	pump.input.lift_switch_2				=	input_data.lift_switch_2;
	pump.input.lift_potentio_ratio			=	input_data.lift_potentiometer_ratio;
	pump.input.tilt_switch					=	input_data.tilt_switch;
	pump.input.aux_switch_1					=	input_data.hyd_aux_switch_1;
	pump.input.aux_switch_2					=	input_data.hyd_aux_switch_2;
	pump.input.seat_switch_1				=	input_data.seat_switch_1;
	pump.input.SOC							=	input_data.battery;
	pump.input.trac_regeneration_current	=	input_data.mL_iBat_Can;		//traction motor battery current (line current)
	pump.input.magnet_ready					=	input_data.mE_StsMagnetized_Can;
	pump.input.motor_torque					=	input_data.mE_mEval_Can;
}

static void control_pump_parameter_update()
{
	pump.parameter.max_rpm							=	service->pump_max_rpm;
	pump.parameter.steer_truck_speed_threshold		=	service->truck_speed_threshold;		// m/h		//threshold to start pump for steering.
	pump.parameter.steer_primary_rpm				=	service->pump_steer_primary_speed;
	pump.parameter.steer_secondary_rpm				=	service->pump_steer_secondary_speed;
	pump.parameter.steer_motor_torque_threshold		=	service->pump_steer_torque_threshold;
	pump.parameter.steer_close_delay				=	service->pump_steer_close_delay;		//*10ms
	pump.parameter.steer_ramp_up					=	service->pump_steer_ramp_up;			//*10ms
	pump.parameter.steer_ramp_down					=	service->pump_steer_ramp_down;			//*10ms

	pump.parameter.lift_mode						=	service->option_lift;
	pump.parameter.lift_1s_rpm						=	service->pump_lift_1s_rpm;
	pump.parameter.lift_2s_rpm_1					=	service->pump_lift_2s_rpm_1;
	pump.parameter.lift_2s_rpm_2					=	service->pump_lift_2s_rpm_2;
	pump.parameter.lift_pot_min_prm					=	service->pump_lift_pot_min_rpm;
	pump.parameter.lift_pot_max_rpm					=	service->pump_lift_pot_max_rpm;
	pump.parameter.lift_ramp_up						=	service->pump_lift_ramp_up;
	pump.parameter.lift_ramp_down					=	service->pump_lift_ramp_down;

	pump.parameter.tilt_rpm							=	service->pump_tilt_rpm;
	pump.parameter.tilt_ramp_up						=	service->pump_tilt_ramp_up;
	pump.parameter.tilt_ramp_down					=	service->pump_tilt_ramp_down;

	pump.parameter.aux1_rpm							=	service->pump_aux1_rpm;
	pump.parameter.aux1_ramp_up						=	service->pump_aux1_ramp_up;
	pump.parameter.aux1_ramp_down					=	service->pump_aux1_ramp_down;

	pump.parameter.aux2_rpm							=	service->pump_aux2_rpm;
	pump.parameter.aux2_ramp_up						=	service->pump_aux2_ramp_up;
	pump.parameter.aux2_ramp_down					=	service->pump_aux2_ramp_down;

	pump.parameter.soc_cutback_start				=	service->pump_soc_reduction_start;
	pump.parameter.soc_cutback_end					=	service->pump_soc_reduction_end;
	pump.parameter.soc_rpm_cutback					=	service->pump_soc_rpm_cutback;

	pump.parameter.motor_discharge_enable			=	service->option_pump_motor_discharge_enable;
	pump.parameter.regenration_current_threshold	=	service->pump_discharge_regen_current_thd;
	pump.parameter.soc_threshold					=	service->pump_discharge_soc_thd;

    pump.parameter.motor_temp_red_start				=	service->pump_motor_temp_red_end;
    pump.parameter.motor_temp_red_end				=	service->pump_motor_temp_red_end;
    pump.parameter.motor_temp_rpm_cutback			=	service->pump_motor_temp_rpm_cutback;

    pump.parameter.chip_temp_red_start				=	service->pump_chip_temp_red_start;
    pump.parameter.chip_temp_red_end				=	service->pump_chip_temp_red_end;
    pump.parameter.chip_temp_rpm_cutback			=	service->pump_chip_temp_rpm_cutback;

	lift_pot_curve_x[0]								=	0;
	lift_pot_curve_x[1]								=	1;
	lift_pot_curve_x[2]								=	99;
	lift_pot_curve_x[3]								=	100;
	lift_pot_curve_y[0]								=	0;
	lift_pot_curve_y[1]								=	pump.parameter.lift_pot_min_prm;
	lift_pot_curve_y[2]								=	pump.parameter.lift_pot_max_rpm;
	lift_pot_curve_y[3]								=	pump.parameter.lift_pot_max_rpm;
	
	pump_parameter_rx(input_data.v_debug_rx_0);
	pump_parameter_tx(output_data.v_debug_4);

	batt_cutback_curve_x[0]						=	0;
	batt_cutback_curve_x[1]						=	pump.parameter.soc_cutback_end;
	batt_cutback_curve_x[2]						=	pump.parameter.soc_cutback_start;
	batt_cutback_curve_x[3]						=	100;
	batt_cutback_curve_y[0]						=	pump.parameter.soc_rpm_cutback;
	batt_cutback_curve_y[1]						=	pump.parameter.soc_rpm_cutback;
	batt_cutback_curve_y[2]						=	100;
	batt_cutback_curve_y[3]						=	100;
}

static void pump_parameter_rx(U16 RxMsgData[4])
{
	U8 ModuleID;
	U8 ParamID;
	
	ModuleID = (U8)RxMsgData[0];
	ParamID = (U8)(RxMsgData[0]>>8);

	if(ModuleID != 0x02u)
	{
	}
	else
	{
		switch (ParamID)
		{
		case LIFT_POT_CURVE_X_0:
			lift_pot_curve_x[0] = RxMsgData[1];
			break;
		case LIFT_POT_CURVE_X_1:
			lift_pot_curve_x[1] = RxMsgData[1];
			break;
		case LIFT_POT_CURVE_X_2:
			lift_pot_curve_x[2] = RxMsgData[1];
			break;
		case LIFT_POT_CURVE_X_3:
			lift_pot_curve_x[3] = RxMsgData[1];
			break;
		case LIFT_POT_CURVE_Y_0:
			lift_pot_curve_y[0] = RxMsgData[1];
			break;
		case LIFT_POT_CURVE_Y_1:
			lift_pot_curve_y[1] = RxMsgData[1];
			break;
		case LIFT_POT_CURVE_Y_2:
			lift_pot_curve_y[2] = RxMsgData[1];
			break;
		case LIFT_POT_CURVE_Y_3:
			lift_pot_curve_y[3] = RxMsgData[1];
			break;
		default:
			break;
		}
	}
}

static void pump_parameter_tx(U16 TxMsgData[4])
{
	static U16 ParaID = 0;

	TxMsgData[0] = 0;
	TxMsgData[0] |= 0x02;
	TxMsgData[0] |= (ParaID<<8);

	switch (ParaID)
	{
	case LIFT_POT_CURVE_X_0:
		TxMsgData[1] = lift_pot_curve_x[0];
		break;
	case LIFT_POT_CURVE_X_1:
		TxMsgData[1] = lift_pot_curve_x[1];
		break;
	case LIFT_POT_CURVE_X_2:
		TxMsgData[1] = lift_pot_curve_x[2];
		break;
	case LIFT_POT_CURVE_X_3:
		TxMsgData[1] = lift_pot_curve_x[3];
		break;

	case LIFT_POT_CURVE_Y_0:
		TxMsgData[1] = lift_pot_curve_y[0];
		break;
	case LIFT_POT_CURVE_Y_1:
		TxMsgData[1] = lift_pot_curve_y[1];
		break;
	case LIFT_POT_CURVE_Y_2:
		TxMsgData[1] = lift_pot_curve_y[2];
		break;
	case LIFT_POT_CURVE_Y_3:
		TxMsgData[1] = lift_pot_curve_y[3];
		break;

	default:
		break;
	}

	ParaID++;
	if(ParaID>7u)
	{
		ParaID = 0;
	}

	TxMsgData[2] = 0;
	TxMsgData[3] = 0;
}

static void control_pump_steer()
{
	if(trilean_true == disable_pump)
	{
		steer_target_rpm = 0;
		steer_current_rpm = 0;
		pumpF.steerFlag = FALSE;

		cnt_1st_rpm_delay = 0;
		cnt_2nd_rpm_delay = 0;
		
		primary_target_rpm = 0;
		secondary_target_rpm = 0;
	}
	else
	{
		/*when push acc. pedal or truck travel speed > 0, start pump for steer*/
		/*to realize primary RPM*/
	
		if((pump.input.acc_pedal_ratio > 0) || (abs(pump.input.truck_speed) >= pump.parameter.steer_truck_speed_threshold))
		{
			pumpF.steerFlag = TRUE;
			primary_target_rpm = pump.parameter.steer_primary_rpm;
			cnt_1st_rpm_delay = pump.parameter.steer_close_delay;		//delay for 2s;
		}
		else
		{
			if(cnt_1st_rpm_delay > 0)
			{
				cnt_1st_rpm_delay--;
			}
			else
			{
				primary_target_rpm = 0;
			}
		}

		/*to realize secondary RPM*/
		if(TRUE == pumpF.steerFlag)
		{
//			if((pump.input.motor_torque > STEER_MOTOR_TORQUE_THRESHOLD)&&(!pumpF.liftFlag)&&(!pumpF.tiltFlag)&&(!pumpF.aux1Flag)&&(!pumpF.aux2Flag))
			if((pump.input.motor_torque > pump.parameter.steer_motor_torque_threshold)&&(!pumpF.liftFlag)&&(!pumpF.tiltFlag)&&(!pumpF.aux1Flag)&&(!pumpF.aux2Flag))
			{
				secondary_target_rpm = pump.parameter.steer_secondary_rpm;
				cnt_2nd_rpm_delay = pump.parameter.steer_close_delay;		//delay for 2s
			}
			else
			{
				if(cnt_2nd_rpm_delay > 0)
				{
					cnt_2nd_rpm_delay--;
				}
				else
				{
					secondary_target_rpm = 0;
				}
			}
		}

		steer_target_rpm = max2_s16(primary_target_rpm, secondary_target_rpm);

		if(steer_target_rpm > pump.output.final_motor_rpm)		//this makes the initial RPM of ramp control starts from final motor_rpm;
		{
			steer_current_rpm = pump.output.final_motor_rpm;
		}
		if(TRUE == pump.input.magnet_ready)
		{
			ramp_control(steer_target_rpm, &steer_current_rpm, pump.parameter.steer_primary_rpm, pump.parameter.steer_ramp_up, pump.parameter.steer_ramp_down);
		}
		if((steer_current_rpm == 0.0f) && (steer_target_rpm == 0))
		{
			pumpF.steerFlag = FALSE;
		}
	}
}

static void control_pump_lift()
{
	S16 ramp_y_value = 0;

	if((trilean_true == disable_pump) || (FALSE == SEAT_SWITCH_ON))		// > 5 means switch is engaged
	{
		lift_target_rpm = 0;
		lift_current_rpm = 0;
		pumpF.liftFlag = FALSE;
	}
	else
	{
		/*3 lift control modes*/
		switch(pump.parameter.lift_mode)
		{
		case one_lift_switch:
			if(TRUE == LIFT_SWITCH_1_ON)		//lift level is operated
			{
				pumpF.liftFlag = TRUE;
				lift_target_rpm = pump.parameter.lift_1s_rpm;
			}
			else
			{
				lift_target_rpm = 0;
			}

			ramp_y_value = pump.parameter.lift_1s_rpm;
			break;

		case two_lift_switch:
			if(TRUE == LIFT_SWITCH_1_ON)		// lift switch 1 is operated
			{
				pumpF.liftFlag = TRUE;

				if(TRUE == LIFT_SWITCH_2_ON)		//lift switch 2 is operated
				{
					lift_target_rpm = pump.parameter.lift_2s_rpm_2;
				}
				else
				{
					lift_target_rpm = pump.parameter.lift_2s_rpm_1;
				}
			}
			else
			{
				lift_target_rpm = 0;
			}

			ramp_y_value = pump.parameter.lift_2s_rpm_2;
			break;

		case potentiometer:
			lift_target_rpm = (S32)interpol_s16_s16(&lift_potentio_curve, pump.input.lift_potentio_ratio);
			if(lift_target_rpm > 0)
			{
				pumpF.liftFlag = TRUE;
			}

			ramp_y_value = pump.parameter.lift_pot_max_rpm;
			break;

		default:
			break;
		}

		if(lift_target_rpm > pump.output.final_motor_rpm)		//this makes the initial RPM of ramp control starts from final motor_rpm;
		{
			lift_current_rpm = pump.output.final_motor_rpm;
		}

		if(TRUE == pump.input.magnet_ready)
		{
			ramp_control(lift_target_rpm, &lift_current_rpm, ramp_y_value, pump.parameter.lift_ramp_up, pump.parameter.lift_ramp_down);
		}

		if((lift_current_rpm == 0.0f) && (lift_target_rpm == 0))
		{
			pumpF.liftFlag = FALSE;
		}
	}
}

static void control_pump_tilt()
{
	if((trilean_true == disable_pump) || (FALSE == SEAT_SWITCH_ON))   //F
	{
		tilt_target_rpm = 0;
		tilt_current_rpm = 0;
		pumpF.tiltFlag = FALSE;
	}
	else
	{
		if(TRUE == TILT_SWITCH_ON)		//F
		{
			pumpF.tiltFlag = TRUE;
			tilt_target_rpm = pump.parameter.tilt_rpm;
		}
		else
		{
			tilt_target_rpm = 0;
		}

		if(tilt_target_rpm > pump.output.final_motor_rpm)		//this makes the initial RPM of ramp control starts from final motor_rpm;
		{
			tilt_current_rpm = pump.output.final_motor_rpm;
		}

		if(TRUE == pump.input.magnet_ready)		//F
		{
			ramp_control(tilt_target_rpm, &tilt_current_rpm, pump.parameter.tilt_rpm, pump.parameter.tilt_ramp_up, pump.parameter.tilt_ramp_down);
		}

		if((tilt_current_rpm == 0.0f) && (tilt_target_rpm == 0))	//T
		{
			pumpF.tiltFlag = FALSE;
		}
	}
}

static void control_pump_aux1()
{
	if((trilean_true == disable_pump) || (FALSE == SEAT_SWITCH_ON))
	{
		aux1_target_rpm = 0;
		aux1_current_rpm = 0;
		pumpF.aux1Flag = FALSE;
	}
	else
	{
		if(TRUE == AUX1_SWITCH_ON)
		{
			pumpF.aux1Flag = TRUE;
			aux1_target_rpm = pump.parameter.aux1_rpm;
		}
		else
		{
			aux1_target_rpm = 0;
		}

		if(aux1_target_rpm > pump.output.final_motor_rpm)		//this makes the initial RPM of ramp control starts from final motor_rpm;
		{
			aux1_current_rpm = pump.output.final_motor_rpm;
		}
		if(TRUE == pump.input.magnet_ready)
		{
			ramp_control(aux1_target_rpm, &aux1_current_rpm, pump.parameter.aux1_rpm, pump.parameter.aux1_ramp_up, pump.parameter.aux1_ramp_down);
		}
		if((aux1_current_rpm == 0.0f) && (aux1_target_rpm == 0))
		{
			pumpF.aux1Flag = FALSE;
		}
	}
}

static void control_pump_aux2()
{
	if((trilean_true == disable_pump) || (FALSE == SEAT_SWITCH_ON))
	{
		aux2_target_rpm = 0;
		aux2_current_rpm = 0;
		pumpF.aux2Flag = FALSE;
	}
	else
	{
		if(TRUE == AUX2_SWITCH_ON)
		{
			pumpF.aux2Flag = TRUE;
			aux2_target_rpm = pump.parameter.aux2_rpm;
		}
		else
		{
			aux2_target_rpm = 0;
		}

		if(aux2_target_rpm > pump.output.final_motor_rpm)		//this makes the initial RPM of ramp control starts from final motor_rpm;
		{
			aux2_current_rpm = pump.output.final_motor_rpm;
		}
		if(TRUE == pump.input.magnet_ready)
		{
			ramp_control(aux2_target_rpm, &aux2_current_rpm, pump.parameter.aux2_rpm, pump.parameter.aux2_ramp_down, pump.parameter.aux2_ramp_down);
		}
		if((aux2_current_rpm == 0.0f) && (aux2_target_rpm == 0))
		{
			pumpF.aux2Flag = FALSE;
		}
	}
}

/*should be called in 10ms cycle*/
static void ramp_control(S16 target_value, F32* current_value, S16 ramp_y, S16 ramp_up_x, S16 ramp_down_x)
{
	if(*current_value < target_value)		//ramp up
	{
		if(ramp_up_x <= 0)
		{
			*current_value = target_value;
		}
		else
		{
			step = (F32)ramp_y / ramp_up_x;
			*current_value += step;
			if(*current_value >= target_value)
			{
				*current_value = target_value;
			}
		}
	}
	else if(*current_value > target_value)		//ramp down
	{
		if(ramp_down_x <= 0)
		{
			*current_value = target_value;
		}
		else
		{
			step = (F32)ramp_y / ramp_down_x;
			*current_value -= step;
			if(*current_value <= target_value)
			{
				*current_value = target_value;
			}
		}
	}
	else
	{

	}
}

static void output_max_rpm()
{
	F32 rv = 0;
	rv = max4_f32(lift_current_rpm, tilt_current_rpm, aux1_current_rpm, aux2_current_rpm);
	rpm_output_buffer = max2_f32(rv, steer_current_rpm);
}

static void interpol_s16_s16_construct( struct interpol_s16_s16_obj *me, pal_size_t count, S16 const * p_x, S16 const * p_y )
{
	me->count= count;
	me->x= p_x;
	me->y= p_y;
}

static void batt_cutback_control()
{
	rpm_output_buffer = (S32)interpol_s16_s16(&batt_cutback_curve, pump.input.SOC) * rpm_output_buffer / 100;
}

static void pump_motor_discharge_control()
{
	if(trilean_true == disable_pump)
	{
		rpm_output_buffer = 0;
		pump.output.final_motor_rpm = 0;
	}
	else
	{
		if(FALSE == pump.parameter.motor_discharge_enable)
		{
			pump.output.final_motor_rpm = rpm_output_buffer;
		}
		else
		{
			if((pump.input.SOC >= pump.parameter.soc_threshold) && (pump.input.trac_regeneration_current >= pump.parameter.regenration_current_threshold))
			{
				switch(pump.parameter.lift_mode)
				{
				case one_lift_switch:
					pump.output.final_motor_rpm = pump.parameter.lift_1s_rpm;
					break;
				case two_lift_switch:
					pump.output.final_motor_rpm = pump.parameter.lift_2s_rpm_2;
					break;
				case potentiometer:
					pump.output.final_motor_rpm = pump.parameter.lift_pot_max_rpm;
					break;
				default:
					break;
				}
			}
			else
			{
				pump.output.final_motor_rpm = rpm_output_buffer;
			}
		}
	}
}

static void pump_motor_enable_control()
{
	if(trilean_true == disable_pump)
	{
		pump_motor_enable = 1;		//motor converter disable;
		pump_motor_PWM_enable = 0;	//motor magnetizing disable;
	}
	else
	{
		pump_motor_enable = 2;		//motor converter enable

		if(pumpF.steerFlag || pumpF.liftFlag || pumpF.tiltFlag || pumpF.aux1Flag || pumpF.aux2Flag)
		{
			cnt2s = 200;					//keep magnetizing for another 2s
			pump_motor_PWM_enable = 1;		//magnetizing enable;
		}
		else
		{
			if(cnt2s>0)
			{
				cnt2s--;
			}
			else
			{
				pump_motor_PWM_enable = 0;
			}
		}
	}
}

/***********************************************************************************************************************
*                             Copyright 2020 Linde Material Handling. All rights reserved.                             *
***********************************************************************************************************************/












